
#include <stdio.h>
#include <stdlib.h>

#include "base/utils/utils.h"
#include "cv/highgui/VideoPOS_Manager.h"
#include "uav/UAS.h"

#include "ComManager.h"
#include "DataManager.h"

using namespace std;
using namespace pi;


////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

ComManager::ComManager()
{
    pi::Thread::setName("ComManager");

    m_uasManager = NULL;
    m_uart = NULL;

    m_nRead = 0;
    m_nWrite = 0;

    m_deviceStatus = COM_DEVICE_CLOSED;
    m_deviceType = COM_DEVICE_UART;

    m_autoSaveFP = NULL;
    m_autoSaveFileName = "";

    // flight auto save path
    string dp = svar.GetString("FastGCS.DataPath", "./Data/FastGCS");
    m_autoSavePath = path_join(dp, "flight_data/fd");

    // valid timestamp value
    int ts_y, ts_m, ts_d;
    ts_y = svar.GetInt("FastGCS.tsValid_Year", 2010);
    ts_m = svar.GetInt("FastGCS.tsValid_Month", 1);
    ts_d = svar.GetInt("FastGCS.tsValid_Day", 1);
    DateTime t(ts_y, ts_m, ts_d);
    m_tsValid = t.toTimeStamp();

    // start flight data transfer
    string fdTrnasAddr = svar.GetString("FastGCS.FlightDataTrans.Addr", "225.0.0.10");
    int fdTransPort = svar.GetInt("FastGCS.FlightDataTrans.Port", 10023);
    m_fdTrans.begin(fdTrnasAddr, fdTransPort);

    // start mission data transfer client/server
    string mdTransAddr = svar.GetString("FastGCS.MissionDataTrans.Addr", "225.0.0.11");
    int mdTransPort = svar.GetInt("FastGCS.MissionDataTrans.Port", 10024);
    m_mdTrans.begin(mdTransAddr, mdTransPort, 0);
    svar.GetPointer("MissionData_Transfer.client.ptr", NULL) = &m_mdTrans;

    if( svar.GetInt("MissionData_Transfer.server.start", 1) ) {
        m_mdTransServer.begin(mdTransAddr, mdTransPort, 1);
        svar.GetPointer("MissionData_Transfer.server.ptr") = &m_mdTransServer;
    }

    // start FlightGear data transfer
    int fgTransPort = svar.GetInt("FastGCS.FlightGearTrans.Port", 5502);
    string fgTransAddr = svar.GetString("FastGCS.FlightGearTrans.IP", "127.0.0.1");
    m_fgTrans.connect(fgTransAddr, fgTransPort);
}

ComManager::~ComManager()
{
    if( m_fdTrans.isRunning() ) m_fdTrans.stop();
    if( m_fgTrans.isRunning() ) m_fgTrans.close();

    m_uasManager = NULL;

    // close connections
    close();

    m_deviceType = COM_DEVICE_NONE;
    m_deviceStatus = COM_DEVICE_CLOSED;
}

void ComManager::threadFunc()
{
    uint8_t             buff_read[1024];
    int                 len, i;

    mavlink_message_t   msg;
    mavlink_status_t    status;

    const int           buff_len_default = 1024;
    uint8_t             buff[buff_len_default];
    int                 buff_len;

    // set global configures
    m_mavlinkMsgShow    = &( svar.GetInt("FastGCS.ShowMavlink", 0) );
    m_playSpeed         = &( svar.GetDouble("FastGCS.PlaySpeed", 1.0) );
    m_playTS            = &( svar.GetDouble("IPC.PlayTS", -1.0) );


    // set time values
    m_tm0 = -1;
    m_st0 = -1;

    // loop until quit
    while( !shouldStop() ) {
        // if device not opened
        if( m_deviceStatus != COM_DEVICE_OPEND ) {
            tm_sleep(80);
            continue;
        }

        // read some chars
        len = read(buff_read, 16);
        if( len <= 0 ) {
            if( len == 0 ) tm_sleep(2);
            else           tm_sleep(80);

            goto THREAD_MAVLINK_WRITE;
        }

        m_nRead += len;

        // save to autosave file
        if( m_autoSaveFP != NULL ) {
            fwrite(buff_read, len, 1, m_autoSaveFP);
            fflush(m_autoSaveFP);
        }

        // parse received data
        for(i=0; i<len; i++) {
            if( mavlink_parse_char(MAVLINK_COMM_0, buff_read[i], &msg, &status) ) {
                // show message
                if( *m_mavlinkMsgShow ) {
                    printf("sysid = %3d, compid = %3d, msgid = %3d, len = %3d, seq = %3d\n",
                           msg.sysid, msg.compid, msg.msgid, msg.len, msg.seq);
                }

                // parse message & send it
                if( m_uasManager != NULL ) {
                    parseMsg(&msg);
                }

                // keep play speed with desired speed
                keepPlaySpeed();
            }
        }

        // play fast forward
        if( m_playFF > 0 ) {
            double tStop = *m_playTS + m_playFF;

            while(1) {
                // read some chars
                len = read(buff_read, 16);
                if( len <= 0 ) {
                    if( len == 0 ) tm_sleep(2);
                    else           tm_sleep(80);
                }

                m_nRead += len;

                // parse received data
                for(i=0; i<len; i++) {
                    if( mavlink_parse_char(MAVLINK_COMM_0, buff_read[i], &msg, &status) ) {
                        // parse message & send it
                        if( m_uasManager != NULL ) {
                            parseMsg(&msg);
                        }
                    }
                }

                if( *m_playTS >= tStop ) break;
            }

            m_playFF = 0;
        }

THREAD_MAVLINK_WRITE:
        // write message to device
        if( m_uasManager != NULL ) {
            buff_len = buff_len_default;
            m_uasManager->get_msg_buff(buff, &buff_len);
            if( buff_len > 0 ) {
                write(buff, buff_len);
                m_nWrite += buff_len;
            }
        }
    }
}

int ComManager::parseMsg(mavlink_message_t *msg)
{
    int i;

    VideoPOS_Manager *vpm = (VideoPOS_Manager*) svar.GetPointer("VideoPOS_Manager.ptr");

    m_uasManager->parseMavlinkMsg(msg);

    // send recent POS data to remote
    UAS_MAV *mav = m_uasManager->get_active_mav();
    if( mav != NULL ) {
        if( m_nLastPOS != mav->m_posData.size() && mav->m_posData.size() > 0 ) {
            m_nLastPOS = mav->m_posData.size();

            // send pos data
            spPOSData pd(new POS_Data);
            *pd = mav->m_posData.back();
            if( vpm != NULL ) vpm->sendPOS(pd);

            // send flight data
            FlightData_Simp fd;

            fd.version      = 1;

            fd.uavType      = mav->uavType;
            fd.uavID        = mav->ID;
            fd.timestamp    = mav->gpDateTime.toTimeStampF();

            // control data
            fd.num_controlInput = svar.GetDouble("UAS.UAV.num_controlInput", 16);
            for(i=0; i<fd.num_controlInput; i++) {
                fd.controlInput[i] = svar.GetDouble(fmt::sprintf("UAS.UAV.controlInput_%d", i), 0);
            }

            // attitude
            fd.lat          = pd->lat;
            fd.lng          = pd->lng;
            fd.alt          = pd->altitude;
            fd.H            = pd->h;
            fd.yaw          = pd->ahrs.yaw;
            fd.pitch        = pd->ahrs.pitch;
            fd.roll         = pd->ahrs.roll;
            fd.driftAngle   = svar.GetDouble("UAS.UAV.driftAngle", 0);

            fd.uavStatus    = svar.GetInt("UAS.UAV.uavStatus", 0);
            fd.flightMode   = mav->customMode;

            fd.gpsHDOP      = mav->HDOP_h;
            fd.gpsSatN      = mav->nSat;

            fd.telemRSSI    = svar.GetDouble("UAS.Telem.RSSI", 100);
            fd.videoRSSI    = svar.GetDouble("UAS.Video.RSSI", 100);

            fd.num_tanks    = svar.GetInt("UAS.UAV.num_tanks", 3);
            for(i=0; i<fd.num_tanks; i++) {
                fd.fuel_quantity[i] = svar.GetDouble(fmt::sprintf("UAS.UAV.fuel_quantity_%d", i), 100);
            }
            fd.battV        = mav->battVolt;

            fd.num_engines  = svar.GetInt("UAS.UAV.num_engines", 4);
            for(i=0; i<fd.num_engines; i++) {
                fd.eng_state[i] = svar.GetInt(fmt::sprintf("UAS.UAV.eng_state_%d", i), 2);
                fd.rpm[i]       = svar.GetDouble(fmt::sprintf("UAS.UAV.rpm_%d", i), 2000);
            }

            fd.landgearState    = svar.GetInt("UAS.UAV.landgearState", 0);

            fd.elevator         = svar.GetDouble("UAS.UAV.elevator", 0);
            fd.left_flap        = svar.GetDouble("UAS.UAV.left_flap", 0);
            fd.right_flap       = svar.GetDouble("UAS.UAV.right_flap", 0);
            fd.left_aileron     = svar.GetDouble("UAS.UAV.left_aileron", 0);
            fd.right_aileron    = svar.GetDouble("UAS.UAV.right_aileron", 0);
            fd.rudder           = svar.GetDouble("UAS.UAV.rudder", 0);
            fd.elevator         = svar.GetDouble("UAS.UAV.elevator", 0);

            m_fdTrans.sendPOS(&fd);

            // if time stamp available then set it
            ri64 ts = pd->time.toTimeStamp();
            if( ts > m_tsValid ) *m_playTS = 1e-6*ts;
        }

        // if reach given height then start video transfer
        if( mav->gpH > svar.GetDouble("FastGCS.SLAM.beginHeight", 50) )
            svar.GetInt("VideoPOS_Transfer.TX.Video", 1) = 1;
        else
            svar.GetInt("VideoPOS_Transfer.TX.Video", 1) = 0;
    }

    return 0;
}

void ComManager::keepPlaySpeed(void)
{
    // check device type
    if( m_deviceType != COM_DEVICE_FILE ) return;

    // get active MAV
    if( m_uasManager == NULL ) return;

    UAS_Base *u = m_uasManager->get_active_mav();
    if( u == NULL ) return;

    if( m_tm0 == -1 ) {
        m_tm0 = u->bootTime;
        m_st0 = tm_get_us();
    } else {
        m_tm1 = u->bootTime;
        m_st1 = tm_get_us();

        int tm_dt = m_tm1 - m_tm0;
        if( tm_dt > 1 ) {
            if( tm_dt < 10000 && tm_dt > 5 ) {
                float dt = 1000.0 * tm_dt / (*m_playSpeed) - (m_st1 - m_st0);
                if( dt > 1 ) tm_sleep_us((ru32)dt);

                m_tm0 = m_tm1;
                m_st0 = m_st1;
            }

            // reset begin time
            if( tm_dt >= 10000 ) {
                m_tm0 = m_tm1;
                m_st0 = m_st1;
            }
        }
    }
}

void ComManager::syncTimeStamp(void)
{
    // get active MAV
    UAS_Base *u = m_uasManager->get_active_mav();
    if( u == NULL ) return;

    // if time stamp available then set it
    ri64 ts = u->gpDateTime.toTimeStamp();
    if( ts > m_tsValid ) *m_playTS = 1.0*ts/1e6;
}

int ComManager::open(ComDeviceType t, std::string devName, int baud)
{
    int ret = -1;

    m_deviceType = t;

    m_nRead = 0;
    m_nWrite = 0;

    m_nLastPOS = 0;
    m_playFF = 0;

    // open communication device
    if( t == COM_DEVICE_UART ) {
        if( m_uart != NULL ) {
            m_uart->close();
            delete m_uart;
            m_uart = NULL;
        }

        StringArray sa = split_text(devName, ":");

        dbg_pt("devName = %s, sa.size = %d", devName.c_str(), sa.size());
        if( sa.size() > 1 ) {
            pi::VirtualUART *vuart = new pi::VirtualUART;
            vuart->setMode(pi::VirtualUART::VUART_NET);

            m_uart = vuart;
        } else {
            m_uart = new pi::UART();
        }

        m_uart->port_name = devName;
        m_uart->baud_rate = baud;

        if( m_uart->open() == 0 ) {
            // set main UART file name
            svar.GetString("FastGCS.UART.mainDEV", "") = devName;

            // generate autosave filename & open it          
            string AutoSavePrjName = svar.GetString("FastGCS.AutoSavePrjName", "autosave");

            if( m_autoSavePath.size() > 0 )
                m_autoSaveFileName = m_autoSavePath + "_" + AutoSavePrjName + ".mvd";
            else
                m_autoSaveFileName = "fd_" + AutoSavePrjName + ".mvd";

            dbg_pi("Open autosave file: %s", m_autoSaveFileName.c_str());
            m_autoSaveFP = fopen(m_autoSaveFileName.c_str(), "wt");
            if( m_autoSaveFP == NULL ) {
                dbg_pw("Can not open autosave file: %s\n", m_autoSaveFileName.c_str());
            }

            // set device status
            m_deviceStatus = COM_DEVICE_OPEND;

            ret = 0;
            goto OPEN_RETURN;
        } else {
            m_deviceStatus = COM_DEVICE_CLOSED;
            goto OPEN_RETURN;
        }
    }

    if( t == COM_DEVICE_IPC ) {
        if( m_uart != NULL ) {
            m_uart->close();
            delete m_uart;
            m_uart = NULL;
        }

        if( m_uart == NULL )
            m_uart = new pi::VirtualUART();


        m_uart->port_name = devName;
        m_uart->baud_rate = baud;

        VirtualUART *vuart = (VirtualUART*) m_uart;
        vuart->setMode(VirtualUART::VUART_IPC);

        if( m_uart->open() == 0 ) {
            // generate autosave filename & open it
            m_autoSaveFileName = pi::auto_filename(m_autoSavePath) + ".mvd";
            dbg_pi("Open autosave file: %s", m_autoSaveFileName.c_str());
            m_autoSaveFP = fopen(m_autoSaveFileName.c_str(), "wt");
            if( m_autoSaveFP == NULL ) {
                dbg_pw("Can not open autosave file: %s\n", m_autoSaveFileName.c_str());
            }

            // set device status
            m_deviceStatus = COM_DEVICE_OPEND;

            ret = 0;
            goto OPEN_RETURN;
        } else {
            m_deviceStatus = COM_DEVICE_CLOSED;
            goto OPEN_RETURN;
        }
    }

    if( t == COM_DEVICE_FILE ) {
        if( m_uart != NULL ) {
            m_uart->close();
            delete m_uart;
            m_uart = NULL;
        }

        if( m_uart == NULL )
            m_uart = new pi::VirtualUART();

        m_uart->port_name = devName;

        VirtualUART *vuart = (VirtualUART*) m_uart;
        vuart->setMode(VirtualUART::VUART_FILE);

        if( m_uart->open() == 0 ) {
            m_deviceStatus = COM_DEVICE_OPEND;
            ret = 0;
            goto OPEN_RETURN;
        } else {
            dbg_pw("Can not open file: %s\n", devName.c_str());
            m_deviceStatus = COM_DEVICE_CLOSED;
            goto OPEN_RETURN;
        }
    }

    if( t == COM_DEVICE_UDP ) {
        dbg_pe("Unsupport UDP communication!");
        goto OPEN_RETURN;
    }

OPEN_RETURN:
    // start receiving thread
    if( 0 == ret ) start();

    return ret;
}

int ComManager::close(void)
{
    // stop Flight data & FlightGear transfer
    //if( m_fdTrans.isRunning() ) m_fdTrans.stop();
    //if( m_fgTrans.isRunning() ) m_fgTrans.close();

    // stop thread
    pi::Thread::stop();
    join(10);

    // reset IO stastics
    m_nRead = 0;
    m_nWrite = 0;

    // close devices
    if( m_deviceType == COM_DEVICE_UART || m_deviceType == COM_DEVICE_IPC ) {
        if( m_deviceStatus == COM_DEVICE_OPEND )
            m_deviceStatus = COM_DEVICE_CLOSED;

        if( m_autoSaveFP != NULL ) {
            fclose(m_autoSaveFP);
            m_autoSaveFP = NULL;
        }

        if( m_uart != NULL ) {
            m_uart->close();
            delete m_uart;
            m_uart = NULL;

            // set main UART file name
            svar.GetString("FastGCS.UART.mainDEV", "") = "";
        }
    }

    if( m_deviceType == COM_DEVICE_FILE ) {
        if( m_deviceStatus == COM_DEVICE_OPEND )
            m_deviceStatus = COM_DEVICE_CLOSED;

        if( m_uart != NULL ) {
            m_uart->close();
            delete m_uart;
            m_uart = NULL;
        }
    }

    if( m_deviceType == COM_DEVICE_UDP ) {
        dbg_pe("Unsupport UDP communication!");
        return -1;
    }

    return 0;
}

int ComManager::seek(double dt)
{
    m_playFF = dt;
    return 0;
}

int ComManager::read(uint8_t *buf, int len)
{
    if( m_deviceStatus != COM_DEVICE_OPEND ) return -1;

    if( m_deviceType == COM_DEVICE_UART ||
        m_deviceType == COM_DEVICE_FILE ||
        m_deviceType == COM_DEVICE_IPC ) {
        return m_uart->read(buf, len);
    }

    return -1;
}

int ComManager::write(uint8_t *buf, int len)
{
    if( m_deviceStatus != COM_DEVICE_OPEND ) return -1;

    if( m_deviceType == COM_DEVICE_UART ||
        m_deviceType == COM_DEVICE_IPC ) {
        return m_uart->write(buf, len);
    }

    return -1;
}
